from __future__ import with_statement

import serial
import threading
from crc import crc
from bidict import bidict
from thread_pipe import ThreadPipe


CPU_USART_START_CHAR = 0x7E
CPU_USART_ESCAPE_CHAR = 0x7D
CPU_USART_XOR_CHAR = 0x20


DATA_COMMANDS = bidict({"no command" : -1,
                        "ok" : 0,
                        "error" : 1,
                        "reset" : 2,
                        "ping" : 3,
                        "pong" : 4,
                        "set duty cycle" : 5,
                        "get rpm" : 6})

def createMessage(command, payload = None):
    data = ""
    data += chr(command)
    if payload is not None:
        data += str(payload)
    data += chr(crc(data))
    escaped_data = ""
    for subdata in data:
        escape_list = [CPU_USART_ESCAPE_CHAR, CPU_USART_START_CHAR]
        if ord(subdata) in escape_list:
            escaped_data += chr(CPU_USART_ESCAPE_CHAR)
            escaped_data += chr(ord(subdata) ^ CPU_USART_XOR_CHAR)
        else:
            escaped_data += subdata
    
    return chr(CPU_USART_START_CHAR) + escaped_data + chr(CPU_USART_START_CHAR)


class DataPackage:
    def __init__(self, command, data):
        self.command = command
        self.data = data
    
    def __repr__(self):
        return "repr" + str(self.data)
    
    def __str__(self):
        return "str" + str(self.data)



class SerialPortRunner(threading.Thread):
    def __init__(self, serial_port):
        self._serial_port = serial_port
        self._data_pipe = None
        self._lock = threading.Lock()
        self._crc_check = True
        self._serial_port = serial.Serial(self._serial_port, 115200, timeout = None)
        self._serial_port.flushInput()
        self._serial_port.flushOutput()
        threading.Thread.__init__(self)

    def setDataPipe(self, data_pipe):
        self._data_pipe = data_pipe
    
    def write(self, message):
        with self._lock:
            print "write", len(message)
            self._serial_port.write(message)
            self._serial_port.flush()
    
    def run(self):
        buff = ""
        escape = False
        while True:
            data = self._serial_port.read()

            for c in data:
                if ord(c) == CPU_USART_START_CHAR:
                    if not escape and ((not self._crc_check and len(buff) > 0) or (self._crc_check and len(buff) > 1)):
                        #print "data, SerialPortRunner (%s): command (%d) datalen (%d)" % (str(time()), ord(buff[0]), len(buff))
                        
                        if self._crc_check:
                            checksum = ord(buff[-1])
                            buff = buff[:-1]

                        # Check crc
                        if self._crc_check and crc(buff) != checksum:
                            print "data, SerialPortRunner, crc fail"
                            print " buffer length", len(buff), crc(buff), checksum
                            print " buffer contents", 
                            for c in buff:
                                print "%03d" % ord(c),
                            print
                        else:
                            command = ord(buff[0])
                            #print "data, SerialPortRunner, got command", command, len(buff)
                            if len(buff) > 1:
                                buff = buff[1:]
                            else:
                                buff = None
                            package = DataPackage(command, buff)
                            if self._data_pipe is not None:
                                self._data_pipe.send(package)
                    
                    buff = ""
                    escape = False
                elif escape:
                    buff += chr(ord(c) ^ CPU_USART_XOR_CHAR)
                    #print "Escaped: ", ord(c), ord(c) ^ CPU_USART_XOR_CHAR
                    escape = False
                elif ord(c) == CPU_USART_ESCAPE_CHAR:
                    escape = True
                else:
                    buff += c


class DataCommunicator:
    def __init__(self, serial_port):
        self.serial_port_runner = SerialPortRunner(serial_port)
        self.serial_port_runner.setDaemon(True)
        self.serial_port_runner.start()
        self._data_pipe, remote_data_pipe = ThreadPipe()
        self.serial_port_runner.setDataPipe(remote_data_pipe)
        self.lock = threading.Lock()
    
    def _putData(self, command, data = None):
        with self.lock:
            message = createMessage(command, data)
            self.serial_port_runner.write(message)
    
    def reset(self):
        print "DataCommunicator, reset"
        if self._serial_port_ok:
            self._putData(DATA_COMMANDS['reset'])
    
    def _sendCommand(self, command, request_response_command = None, timeout = None):
        if timeout is None:
            timeout = 0.2
        # Clear pipe
        while self._data_pipe.recv(0) is not None:
            pass
        #print "_sendCommand", command, DATA_COMMANDS[command]
        self._putData(DATA_COMMANDS[command])
        if request_response_command is not None:
            package = self._data_pipe.recv(timeout)
            if package is not None and package.command == DATA_COMMANDS[request_response_command]:
                return package
        return None
    
    def ping(self, timeout = None):
        package = self._sendCommand("ping", request_response_command = "pong")
        print "DataCommunicator, ping", package
        if package is not None:
            return True
        return False
    
    def send(self, command, data = None):
        self._putData(DATA_COMMANDS[command], data)

    def recv(self):
        return self._data_pipe.recv(0.5)
    
        
if __name__ == "__main__":
    dc = DataCommunicator("/dev/ttyUSB0")

    # VARNING nar serieporten oppnas maste man forst oppna en annan baudrate, sedan den man vill ha
    dc.ping()

    while True:
        c = raw_input('--> ')
        if c == "quit" or c == "q":
            quit()
        elif c == "rpm":
            dc.send("get rpm")
            print dc.recv()
        else:
            dc.send("set duty cycle", int(c))
            print "Setting duty cycle" + str(int(c))

